import numpy as np
    
def projectPoints(X, K, R, t, Kd):
    """ Projects points X (3xN) using camera intrinsics K (3x3),
    extrinsics (R,t) and distortion parameters Kd=[k1,k2,p1,p2,k3].
    
    Roughly, x = K*(R*X + t) + distortion
    
    See http://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
    or cv2.projectPoints
    """
    
    x = np.asarray(R*X + t)
    
    x[0:2,:] = x[0:2,:]/x[2,:]
    
    r = x[0,:]*x[0,:] + x[1,:]*x[1,:]
    
    x[0,:] = x[0,:]*(1 + Kd[0]*r + Kd[1]*r*r + Kd[4]*r*r*r) + 2*Kd[2]*x[0,:]*x[1,:] + Kd[3]*(r + 2*x[0,:]*x[0,:])
    x[1,:] = x[1,:]*(1 + Kd[0]*r + Kd[1]*r*r + Kd[4]*r*r*r) + 2*Kd[3]*x[0,:]*x[1,:] + Kd[2]*(r + 2*x[1,:]*x[1,:])

    x[0,:] = K[0,0]*x[0,:] + K[0,1]*x[1,:] + K[0,2]
    x[1,:] = K[1,0]*x[0,:] + K[1,1]*x[1,:] + K[1,2]
    
    return x


def get_uniform_camera_order():
    """ Returns uniformly sampled camera order as a list of tuples [(panel,node), (panel,node), ...]."""
    panel_order =[1,19,14,6,16,9,5,10,18,15,3,8,4,20,11,13,7,2,17,12,9,5,6,3,15,2,12,14,16,10,4,13,20,8,17,19,18,9,4,6,1,20,1,11,7,7,14,15,3,2,16,13,3,15,17,9,20,19,8,11,5,8,18,10,12,19,5,6,16,12,4,6,20,13,4,10,15,12,17,17,16,1,5,3,2,18,13,16,8,19,13,11,10,7,3,2,18,10,1,17,10,15,14,4,7,9,11,7,20,14,1,12,1,6,11,18,7,8,9,3,15,19,4,16,18,1,11,8,4,10,20,13,6,16,7,6,16,17,12,5,17,4,8,20,12,17,14,2,19,14,18,15,11,11,9,9,2,13,5,15,20,18,8,3,19,11,9,2,13,14,5,9,17,9,7,6,12,16,18,17,13,15,17,20,4,2,2,12,4,1,16,4,11,1,16,12,18,9,7,20,1,10,10,19,5,8,14,8,4,2,9,20,14,17,11,3,12,3,13,6,5,16,3,5,10,19,1,11,13,17,18,2,5,14,19,15,8,8,9,3,6,16,15,18,20,4,13,2,11,20,7,13,15,18,10,20,7,5,2,15,6,13,4,17,7,3,19,19,3,10,2,12,10,7,7,12,11,19,8,9,6,10,6,15,10,11,3,16,1,5,14,6,5,13,20,14,4,18,10,14,14,1,19,8,14,19,3,6,6,3,13,17,8,20,15,18,2,2,16,5,19,15,9,12,19,17,8,9,3,7,1,12,7,13,1,14,5,12,11,2,16,1,18,4,18,10,16,11,7,5,1,16,9,4,15,1,7,10,14,3,2,17,13,19,20,15,10,4,8,16,14,5,6,20,12,5,18,7,1,8,11,5,13,1,16,14,18,12,15,2,12,3,8,12,17,8,20,9,2,6,9,6,12,3,20,15,20,13,3,14,1,4,8,6,10,7,17,13,18,19,10,20,12,19,2,15,10,8,19,11,19,11,2,4,6,2,11,8,7,18,14,4,12,14,7,9,7,11,18,16,16,17,16,15,4,15,9,17,13,3,6,17,17,20,19,11,5,3,1,18,4,10,5,9,13,1,5,9,6,14]
    node_order = [1,14,3,15,12,12,8,6,13,12,12,17,7,17,21,17,4,6,12,18,2,18,5,4,2,17,12,10,18,8,18,5,10,10,17,1,18,7,12,9,13,5,6,18,16,9,16,8,8,10,21,22,16,16,21,16,14,6,14,11,11,20,4,22,4,22,20,19,15,15,15,12,2,2,3,3,20,22,5,9,3,16,23,22,20,8,8,9,2,16,14,16,16,14,1,13,16,12,10,15,18,6,13,10,7,10,4,1,7,21,8,6,4,7,9,10,11,8,4,6,10,4,5,6,21,21,6,6,19,20,20,20,14,19,22,22,23,19,9,15,23,23,23,23,19,2,8,2,8,19,19,23,23,19,19,23,24,24,2,14,12,2,12,14,12,2,14,15,11,6,6,21,4,5,5,4,2,10,5,10,7,3,7,9,8,9,3,7,9,9,7,2,5,5,5,5,7,8,8,4,7,11,9,7,5,3,5,7,6,8,9,8,7,8,8,3,8,7,6,11,7,2,9,9,2,11,12,7,4,6,6,7,4,4,9,18,1,5,6,5,10,11,5,9,6,11,12,1,10,11,6,9,7,11,5,1,2,12,11,11,3,3,21,11,10,2,3,10,11,19,5,11,13,12,20,13,3,5,9,11,8,4,6,4,7,12,10,8,11,19,14,23,10,1,3,12,4,3,10,9,2,3,20,4,11,2,20,20,2,23,10,3,22,22,1,12,12,21,4,22,23,22,18,10,18,22,11,3,18,13,18,3,3,13,2,1,3,20,20,4,20,14,14,20,20,14,14,22,18,21,20,22,20,22,9,22,21,21,22,21,22,20,21,21,21,21,23,17,21,13,20,13,13,15,17,1,23,23,23,18,13,16,15,19,17,17,22,21,17,14,1,13,13,14,14,16,19,17,18,1,13,18,24,19,16,13,18,18,15,23,17,14,19,17,1,19,13,19,1,15,17,13,23,13,19,24,15,15,19,15,17,1,16,24,21,23,14,24,15,24,24,1,16,15,24,1,17,17,15,24,1,16,16,19,13,15,22,24,23,17,16,18,1,24,24,24,17,24,24,17,16,24,14,15,16,15,24,24,24,18]
    
    return zip(panel_order, node_order)